/*
 * Copyright (c) 2015-2016, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/***** Includes *****/

#include <xdc/std.h>
#include <xdc/runtime/System.h>

#include <ti/sysbios/BIOS.h>

#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Event.h>

#ifdef USE_BIM
#include <ti/sysbios/knl/Clock.h>
#include "bim/BimFactoryReset.h"
#endif

/* Drivers */
#include <ti/drivers/PIN.h>
#include <ti/drivers/pin/PINCC26XX.h>
#include <ti/display/Display.h>
#include <ti/display/DisplayExt.h>

/* Board Header files */
#include "Board.h"

#include "ConcentratorTask.h"
#include "wsnprotocol/WsnProtocol.h"

#include "oad/uart_serial_download.h"
#include "oad/wsn_oad_server.h"
#include "oad/oad_target.h"


/***** Defines *****/
#define CONCENTRATOR_TASK_STACK_SIZE 1024
#define CONCENTRATOR_TASK_PRIORITY   3

#define CONCENTRATOR_EVENT_ALL                     0xFFFFFFFF
#define CONCENTRATOR_EVENT_UPDATE_DISPLAY      (uint32_t)(1 << 0)
#define CONCENTRATOR_EVENT_MESSAGE_PENDING         (uint32_t)(1 << 1)
#define CONCENTRATOR_EVENT_NODE_BTN               (uint32_t)(1 << 2)
#define CONCENTRATOR_EVENT_OAD_BLOCK_SENT          (uint32_t)(1 << 3)
#define CONCENTRATOR_EVENT_USD_START               (uint32_t)(1 << 4)
#ifdef USE_BIM
#define CONCENTRATOR_EVENT_FACTORY_RESET           (uint32_t)(1 << 5)
#endif

#define CONCENTRATOR_MAX_NODES 6

#define CONCENTRATOR_DISPLAY_LINES 8

#define CONCENTRATOR_NODE_REQ_ATTEMPTS 3

#ifdef USE_BIM
/* BLE Load button check timers - check button ever 100ms for 5s*/
#define NODE_BLE_BOOTLODER_BUTTON_CHECK_TIMER_MS    100
#define NODE_BLE_BOOTLODER_BUTTON_CHECK_DURATION_S  6
#endif

#define CONCENTRATOR_OAD_LED Board_LED1
#define CONCENTRATOR_ACTIVITY_LED Board_LED0

//TODO: change to enum
#define NODE_STATE_NEW                  0
#define NODE_STATE_GET_FW_VER           1
#define NODE_STATE_GET_FW_VER_FAILED    2
#define NODE_STATE_REQ_FW_UPDATE        3
#define NODE_STATE_REQ_FW_UPDATE_FAILED 4
#define NODE_STATE_FW_UPDATING          5
#define NODE_STATE_FW_UPDATING_FAILED   6
#define NODE_STATE_RUNNING              7

/***** Type declarations *****/

struct AdcSensorNode {
    uint8_t address;
    char fwVersion[WSNPROTOCOL_FW_VERSION_STR_LEN];
    uint8_t nodeState;
    uint8_t nodeReqAttempts;
    uint16_t latestAdcValue;
    uint8_t button;
    int8_t latestRssi;
};

struct MessagePending {
    uint8_t address;
    uint8_t packetId;
};

struct MessagePending messagePending = {0};
char availableFwVersion[WSNPROTOCOL_FW_VERSION_STR_LEN] = "None";

/*
 * Application button pin configuration table:
 *   - Buttons interrupts are configured to trigger on falling edge.
 */
PIN_Config buttonPinTable[] = {
    Board_BUTTON0  | PIN_INPUT_EN | PIN_PULLUP | PIN_IRQ_NEGEDGE,
    Board_BUTTON1  | PIN_INPUT_EN | PIN_PULLUP | PIN_IRQ_NEGEDGE,
    PIN_TERMINATE
};

/* Configure LED Pin */
PIN_Config oadLedPinTable[] = {
    CONCENTRATOR_ACTIVITY_LED | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    CONCENTRATOR_OAD_LED | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    PIN_TERMINATE
};

/***** Variable declarations *****/
static Task_Params concentratorTaskParams;
Task_Struct concentratorTask;    /* not static so you can see in ROV */
static uint8_t concentratorTaskStack[CONCENTRATOR_TASK_STACK_SIZE];
Event_Struct concentratorEvent;  /* not static so you can see in ROV */
static Event_Handle concentratorEventHandle;
struct AdcSensorNode knownSensorNodes[CONCENTRATOR_MAX_NODES];
static uint8_t selectedNode = 0;
static Display_Handle hDisplayLcd;
static Display_Handle hDisplaySerial;
static PIN_Handle buttonPinHandle;
static PIN_State buttonPinState;

static bool fwOadInProgress = false;
static bool uartFwUpdateInProgress = false;
static uint32_t fwOadBlockTotal = 0;


#ifdef USE_BIM
/* Clock for the fast report timeout */
Clock_Struct bleBootloadBtnCheckClock;     /* not static so you can see in ROV */
static Clock_Handle bleBootloadBtnCheckClockHandle;
#endif

/* Pin driver handle */
static PIN_Handle ledPinHandle;
static PIN_State ledPinState;

/***** Prototypes *****/
static void concentratorTaskFunction(UArg arg0, UArg arg1);
static void updateLcd(void);
static uint8_t addNewNode(uint8_t address);
static uint8_t getNodeIdx(uint8_t address);
void buttonCallback(PIN_Handle handle, PIN_Id pinId);

static void startLocalNodeFwUpdate(void);
static void endLocalNodeFwUpdate(int32_t status);
static void processLocalNodeFwUpdate(uint32_t events);

static void remoteNodeFwUpdate_start(void);
static void remoteNodeFwUpdate_nextBlock(uint8_t nodeAddr, uint16_t blockNum);

static void adcPacketCb(uint8_t srcAddr, int16_t adcData);
static void dmSensorPacketCb(uint8_t srcAddr, WSNProtocol_DmSensorData_t dmSensorData);
static void fwVersionRspCb(uint8_t srcAddr, char *fwVersionStr);
static void oadImgIdentifyRspCb(uint8_t srcAddr, uint8_t status);
static void oadBlockReqCb(uint8_t srcAddr, uint16_t blkNum);
static void errorCb(uint8_t dstAddr, uint8_t expectedRsp, WSNProtocol_Status_t status);

WSNProtocol_packetCBs_t wsnProtocolCbs = {
    NULL,                //Incoming Ack
    adcPacketCb,         //Incoming Adc packet
    dmSensorPacketCb,    //Incoming Dual Mode Adc Packet
    NULL,                //Incoming FW Req
    fwVersionRspCb,      //Incoming FW Version Rsp
    NULL,                //Incoming Image Identify Req
    oadImgIdentifyRspCb, //Incoming Image Identify Rsp
    oadBlockReqCb,     //Incoming OAD Block Req
    NULL,                //Incoming OAD Block Rsp
    errorCb,            //Error callback
};

#ifdef USE_BIM
void static bleBtnCheckTimerCallback(UArg arg0);
#endif

/***** Function definitions *****/
void ConcentratorTask_init(void) {

    /* Create event used internally for state changes */
    Event_Params eventParam;
    Event_Params_init(&eventParam);
    Event_construct(&concentratorEvent, &eventParam);
    concentratorEventHandle = Event_handle(&concentratorEvent);

#ifdef USE_BIM
    /* Create clock object which is used for ble bootload button check */
    Clock_Params clkParams;
    clkParams.period = 0;
    clkParams.startFlag = FALSE;
    Clock_construct(&bleBootloadBtnCheckClock, bleBtnCheckTimerCallback, 1, &clkParams);
    bleBootloadBtnCheckClockHandle = Clock_handle(&bleBootloadBtnCheckClock);
#endif

    /* Create the concentrator radio protocol task */
    Task_Params_init(&concentratorTaskParams);
    concentratorTaskParams.stackSize = CONCENTRATOR_TASK_STACK_SIZE;
    concentratorTaskParams.priority = CONCENTRATOR_TASK_PRIORITY;
    concentratorTaskParams.stack = &concentratorTaskStack;
    Task_construct(&concentratorTask, concentratorTaskFunction, &concentratorTaskParams, NULL);

    /* initialise the wsn protocol task */
    WSNProtocol_init();
}

static void concentratorTaskFunction(UArg arg0, UArg arg1)
{
    img_hdr_t remoteAppImageHdr;

    /* Set Default parameters structure */
    static WSNProtocol_Params_t WSNProtocol_params;

    /* Initialize and open the Wsn Protocol Task */
    WSNProtocol_Params_init(&WSNProtocol_params);
    WSNProtocol_params.pCallbacks = &wsnProtocolCbs;
    WSNProtocol_params.devType = WSNProtocol_DevType_Concentrator;
    WSNProtocol_params.devAddress = WSNPROTOCOL_CONCENTRATOR_ADDRESS;
    WSNProtocol_open(&WSNProtocol_params);

    /* get Available FW version*/
    OADTarget_getRemoteImageHeader(&remoteAppImageHdr);
    if ((remoteAppImageHdr.ver != 0xFFFF) || (remoteAppImageHdr.ver == 0))
    {
        sprintf(availableFwVersion,"rfWsnNode v%02d.%02d.00", ((remoteAppImageHdr.ver & 0xFF00)>>8), (remoteAppImageHdr.ver & 0xFF));
    }

    /* Initialize display and try to open both UART and LCD types of display. */
    Display_Params params;
    Display_Params_init(&params);
    params.lineClearMode = DISPLAY_CLEAR_BOTH;

    /* Open both an available LCD display and an UART display.
     * Whether the open call is successful depends on what is present in the
     * Display_config[] array of the board file.
     *
     * Note that for SensorTag evaluation boards combined with the SHARP96x96
     * Watch DevPack, there is a pin conflict with UART such that one must be
     * excluded, and UART is preferred by default. To display on the Watch
     * DevPack, add the precompiler define BOARD_DISPLAY_EXCLUDE_UART.
     */
    hDisplayLcd = Display_open(Display_Type_LCD, &params);
    hDisplaySerial = Display_open(Display_Type_UART, &params);

    /* Check if the selected Display type was found and successfully opened */
    if (hDisplaySerial)
    {
        Display_print0(hDisplaySerial, 0, 0, "\033[2J \033Waiting for nodes...");
        Display_print1(hDisplaySerial, 1, 0, "*Available FW: %s", availableFwVersion);
        Display_print0(hDisplaySerial, 1, 0, "Press BTN-1 to update Remote App FW over UART");
    }

    /* Check if the selected Display type was found and successfully opened */
    if (hDisplayLcd)
    {
        Display_print0(hDisplayLcd, 0, 0, "Waiting for nodes...");
        Display_print1(hDisplayLcd, 1, 0, "*Available FW: %s", availableFwVersion);
        Display_print0(hDisplaySerial, 1, 0, "Press BTN-1 to update Remote App FW over UART");
    }

#ifdef USE_BIM
    /* setup timer for ble bootload button check */
    Clock_setTimeout(bleBootloadBtnCheckClockHandle,
            NODE_BLE_BOOTLODER_BUTTON_CHECK_TIMER_MS * 1000 / Clock_tickPeriod);
#endif

    buttonPinHandle = PIN_open(&buttonPinState, buttonPinTable);
    if(!buttonPinHandle)
    {
        System_abort("Error initializing button pins\n");
    }

    /* Setup callback for button pins */
    if (PIN_registerIntCb(buttonPinHandle, &buttonCallback) != 0)
    {
        System_abort("Error registering button callback function");
    }


    /* Open LED pins */
    ledPinHandle = PIN_open(&ledPinState, oadLedPinTable);
    if (!ledPinHandle)
    {
        System_abort("Error initializing board 3.3V domain pins\n");
    }

    /* set selected node to 0 */
    selectedNode = 0;

    /* Enter main task loop */
    while(1) {
        /* Wait for event */
        uint32_t events = Event_pend(concentratorEventHandle, 0, CONCENTRATOR_EVENT_ALL, BIOS_WAIT_FOREVER);

        /* If we got a new ADC sensor value */
        if(events & CONCENTRATOR_EVENT_UPDATE_DISPLAY) {

            /* Update the values on the LCD */
            updateLcd();
        }

        if(events & USD_EVENT)
        {
            processLocalNodeFwUpdate(events);
        }

        if(events & CONCENTRATOR_EVENT_USD_START)
        {
            startLocalNodeFwUpdate();
        }

        if(events & CONCENTRATOR_EVENT_NODE_BTN)
        {
            if(WSNProtocol_isPending())
            {
                WSNProtocol_cancelPending();
            }
            else
            {
                if(fwOadInProgress)
                {
                    WOS_endFwUpdate();

                    Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_OPEN, NULL);
                    fwOadInProgress = false;
                }
                else
                {
                    /* Start node FW OAD */
                    remoteNodeFwUpdate_start();
                }
            }

            /* Update the values on the LCD */
            updateLcd();
        }

        if(events & CONCENTRATOR_EVENT_OAD_BLOCK_SENT)
        {
            static uint32_t cnt = 0;

            if(cnt++ > 10)
            {
                PIN_setOutputValue(ledPinHandle, CONCENTRATOR_OAD_LED,!PIN_getOutputValue(Board_LED1));
                cnt = 0;
            }

            /* Update the values on the LCD */
            updateLcd();
        }

#ifdef USE_BIM
        if (events & CONCENTRATOR_EVENT_FACTORY_RESET) {

            Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_CLOSE, NULL);

            BimFactoryReset_applyFactoryImage();

            /* BIM Factory Reset failed, re-enable the LCD */
            Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_OPEN, NULL);

            /* Re-enable interrupts to detect button release. */
            PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON0 | PIN_IRQ_NEGEDGE);
            PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON1 | PIN_IRQ_NEGEDGE);
        }
#endif
    }
}

static void startLocalNodeFwUpdate(void)
{
    /* update local image */
    if(!uartFwUpdateInProgress)
    {
        uartFwUpdateInProgress = true;

        //Free LCD display for SPI driver
        Display_print0(hDisplayLcd, 0, 0, "Waiting for Node FW update...");
        Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_CLOSE, NULL);

        //Free Serial display for UART Driver
        Display_print0(hDisplaySerial, 0, 0, "\033[2J \033\rWaiting for Node FW update...");
        Display_close(hDisplaySerial);

        USD_open(concentratorEventHandle);

        USD_startFwUpdate();
    }
}

static void processLocalNodeFwUpdate(uint32_t events)
{
    static uint32_t cnt = 0;
    int32_t status;

    status = USD_processFwUpdate(events);
    if(cnt++ > 10)
    {
        PIN_setOutputValue(ledPinHandle, CONCENTRATOR_OAD_LED,!PIN_getOutputValue(Board_LED1));
        cnt = 0;
    }

    if(status != 0)
    {
        endLocalNodeFwUpdate(status);
    }
}

static void endLocalNodeFwUpdate(int32_t status)
{
    img_hdr_t remoteAppImageHdr;
    Display_Params params;
    Display_Params_init(&params);
    params.lineClearMode = DISPLAY_CLEAR_BOTH;

    USD_close();

    /* get Available FW version*/
    OADTarget_getRemoteImageHeader(&remoteAppImageHdr);
    if ((remoteAppImageHdr.ver != 0xFFFF) || (remoteAppImageHdr.ver == 0))
    {
        sprintf(availableFwVersion,"rfWsnNode v%02d.%02d.00", ((remoteAppImageHdr.ver & 0xFF00)>>8), (remoteAppImageHdr.ver & 0xFF));
    }

    Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_OPEN, NULL);
    hDisplaySerial = Display_open(Display_Type_UART, &params);

    uartFwUpdateInProgress = false;

    if(status)
    {
        Display_print0(hDisplaySerial, 0, 0, "\r\033[2J \033FW Update Successful");
    }
    else
    {
        Display_print0(hDisplaySerial, 0, 0, "\r\033[2J \033FW Update Failed");
    }

}

static void remoteNodeFwUpdate_start(void)
{
    if( ((!fwOadInProgress) && (selectedNode > 0) ) &&
        (knownSensorNodes[selectedNode-1].nodeState == NODE_STATE_RUNNING) )

    {
        uint8_t nodeAddr;
        knownSensorNodes[selectedNode-1].nodeState = NODE_STATE_REQ_FW_UPDATE;

        Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);

        //Free LCD display for SPI driver
        Display_print0(hDisplayLcd, 0, 0, "Waiting for Node FW update...");
        Display_print0(hDisplayLcd, 1, 0, "Press BTN-2 to cancel FW update");
        Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_CLOSE, NULL);

        fwOadInProgress = true;
        nodeAddr = knownSensorNodes[selectedNode-1].address;
        fwOadBlockTotal = WOS_startFwUpdate(nodeAddr);

        fwOadBlockTotal = fwOadBlockTotal - 1;
    }
}

static void remoteNodeFwUpdate_nextBlock(uint8_t nodeAddr, uint16_t blockNum)
{
    if (fwOadInProgress)
    {
        WOS_nextBlock(nodeAddr, blockNum);

        if(blockNum == fwOadBlockTotal)
        {
            WOS_endFwUpdate();

            Display_control(hDisplayLcd, DISPLAY_CMD_TRANSPORT_OPEN, NULL);
            fwOadInProgress = false;
        }
    }
    else
    {
        uint8_t nodeIdx;

        //Send block num 0xFFFF to abort OAD on node
        uint8_t blockBuff[OAD_BLOCK_SIZE + 2] = {0};
        blockBuff[0] = 0xFF;
        blockBuff[1] = 0xFF;

        WSNProtocol_sendOadImgBlockRsp(nodeAddr, blockBuff);

        nodeIdx = getNodeIdx(nodeAddr);

        knownSensorNodes[nodeIdx].nodeState = NODE_STATE_NEW;
    }
    //If we are not fwOadInProgress ignore the request
}

static void adcPacketCb(uint8_t srcAddr, int16_t adcData)
{
    /* get/create node index*/
    uint8_t nodeIdx = getNodeIdx(srcAddr);

    if (nodeIdx == CONCENTRATOR_MAX_NODES)
    {
        nodeIdx = addNewNode(srcAddr);
    }

    /* Save the values */
    knownSensorNodes[nodeIdx].latestAdcValue = adcData;
    knownSensorNodes[nodeIdx].button = 0; //no button value in ADC packet
    knownSensorNodes[nodeIdx].latestRssi = WSNProtocol_getLastRssi();

    Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);

    PIN_setOutputValue(ledPinHandle, CONCENTRATOR_ACTIVITY_LED,!PIN_getOutputValue(CONCENTRATOR_ACTIVITY_LED));

}

static void dmSensorPacketCb(uint8_t srcAddr, WSNProtocol_DmSensorData_t dmSensorData)
{
    /* get/create node index*/
    uint8_t nodeIdx = getNodeIdx(srcAddr);

    if (nodeIdx == CONCENTRATOR_MAX_NODES)
    {
        nodeIdx = addNewNode(srcAddr);
    }

    /* Save the values */
    knownSensorNodes[nodeIdx].latestAdcValue = dmSensorData.adcValue;
    knownSensorNodes[nodeIdx].button = dmSensorData.button;
    knownSensorNodes[nodeIdx].latestRssi = WSNProtocol_getLastRssi();

    Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);

    PIN_setOutputValue(ledPinHandle, CONCENTRATOR_ACTIVITY_LED,!PIN_getOutputValue(CONCENTRATOR_ACTIVITY_LED));

    //TODO: use rsp timeout callback to know if it was not sent and re-request
    switch (knownSensorNodes[nodeIdx].nodeState)
    {
    case NODE_STATE_NEW:
        knownSensorNodes[nodeIdx].nodeReqAttempts = 0;

        if (WSNProtocol_sendFwVersionReq(srcAddr) == WSNProtocol_Status_Success)
        {
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_GET_FW_VER;
            /* Update nodes FW to show progress */
            System_sprintf(&knownSensorNodes[nodeIdx].fwVersion, "Requested FW version");
        }
        else
        {
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_GET_FW_VER_FAILED;
        }

        break;
    case NODE_STATE_GET_FW_VER_FAILED:
        if (knownSensorNodes[nodeIdx].nodeReqAttempts < CONCENTRATOR_NODE_REQ_ATTEMPTS)
        {
            if(!WSNProtocol_isPending())
            {
                knownSensorNodes[nodeIdx].nodeReqAttempts++;
                if (WSNProtocol_sendFwVersionReq(srcAddr) == WSNProtocol_Status_Success)
                {
                    knownSensorNodes[nodeIdx].nodeState = NODE_STATE_GET_FW_VER;
                }
                else
                {
                    knownSensorNodes[nodeIdx].nodeState = NODE_STATE_GET_FW_VER_FAILED;
                }
            }
        }

        break;
    }
}

static void fwVersionRspCb(uint8_t srcAddr, char *fwVersionStr)
{
    uint8_t nodeIdx = getNodeIdx(srcAddr);

    if (nodeIdx < CONCENTRATOR_MAX_NODES)
    {
        /* Save the values */
        memcpy(knownSensorNodes[nodeIdx].fwVersion, fwVersionStr, WSNPROTOCOL_FW_VERSION_STR_LEN);

        knownSensorNodes[nodeIdx].nodeState = NODE_STATE_RUNNING;

        Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);
    }
}

static void oadImgIdentifyRspCb(uint8_t srcAddr, uint8_t status)
{
    uint8_t nodeIdx = getNodeIdx(srcAddr);

    if(knownSensorNodes[nodeIdx].nodeState == NODE_STATE_REQ_FW_UPDATE)
    {
        if(status == 1)
        {
            //Image header was accepted
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_FW_UPDATING;
        }
        else
        {
            //Image header was rejected
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_REQ_FW_UPDATE_FAILED;
        }
    }

    Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);
}

static void oadBlockReqCb(uint8_t srcAddr, uint16_t blkNum)
{
    char blockReqStr[16];
    uint8_t nodeIdx = getNodeIdx(srcAddr);

    sprintf(&blockReqStr, "Block %04x/%04x\0", blkNum, fwOadBlockTotal);

    /* Update nodes FW to show progress */
    memcpy(knownSensorNodes[nodeIdx].fwVersion, blockReqStr, sizeof(blockReqStr));

    /* Send node next FW OAD block*/
    remoteNodeFwUpdate_nextBlock(srcAddr, blkNum);

    Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_OAD_BLOCK_SENT);
}

static void errorCb(uint8_t dstAddr, uint8_t expectedRsp, WSNProtocol_Status_t status)
{
    uint8_t nodeIdx = getNodeIdx(dstAddr);

    if (nodeIdx < CONCENTRATOR_MAX_NODES)
    {
        if ((knownSensorNodes[nodeIdx].nodeState == NODE_STATE_GET_FW_VER) &&
             (expectedRsp == WSNPROTOCOL_PACKET_TYPE_FW_VERSION_RSP) )
        {
            /* Fw Version Req timed out  or canceled*/
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_GET_FW_VER_FAILED;

            /* Update nodes FW to show progress*/
            sprintf(&knownSensorNodes[nodeIdx].fwVersion, "fwVersionReq failed attempt %d01", knownSensorNodes[nodeIdx].nodeReqAttempts);

            Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);
        }
        if ((knownSensorNodes[nodeIdx].nodeState == NODE_STATE_REQ_FW_UPDATE) &&
             (expectedRsp == WSNPROTOCOL_PACKET_TYPE_OAD_IMG_IDENTIFY_RSP) )
        {
            /* Fw Update Req timed out or canceled */
            knownSensorNodes[nodeIdx].nodeState = NODE_STATE_REQ_FW_UPDATE_FAILED;

            /* Update nodes FW to show progress */
            sprintf(&knownSensorNodes[nodeIdx].fwVersion, "fwVersionReq failed attempt %d01", knownSensorNodes[nodeIdx].nodeReqAttempts);

            Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);
        }
    }
}


static uint8_t getNodeIdx(uint8_t address) {

    uint8_t nodeIdx = CONCENTRATOR_MAX_NODES;
    uint8_t i;

    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++)
    {
        if (knownSensorNodes[i].address == address)
        {
            nodeIdx = i;
            break;
        }
    }

    return nodeIdx;
}

static uint8_t addNewNode(uint8_t address) {
    static uint8_t lastNodeIdx = 0;
    uint8_t newNodeIdx;

    /* Set version string to unknown */
    char unknownVersion[] = "Unknown";
    memcpy(knownSensorNodes[lastNodeIdx].fwVersion, unknownVersion, sizeof(unknownVersion));

    /* save node address */
    knownSensorNodes[lastNodeIdx].address = address;

    knownSensorNodes[lastNodeIdx].nodeState = NODE_STATE_NEW;

    newNodeIdx = lastNodeIdx;

    /* Increment and wrap */
    lastNodeIdx++;
    if (lastNodeIdx == CONCENTRATOR_MAX_NODES)
    {
        lastNodeIdx = 0;
    }

    return newNodeIdx;
}

static void updateLcd(void) {
    struct AdcSensorNode* nodePointer = knownSensorNodes;
    uint8_t currentLcdLine;
    char selectedChar;
    char fwUpdatingStr[] = "Updating, waiting for node to wake...\0";
    char fwUpateFailedStr[] = "FW: Update Failed\0";

    //Only update display if drives are free
    if(!uartFwUpdateInProgress)
    {
        /* is available FW selected */
        if (selectedNode == 0)
        {
            selectedChar = '*';
        }
        else
        {
            selectedChar = ' ';
        }

        /* clear display and print stored node FW version LCD */
        Display_clear(hDisplayLcd);
        Display_print2(hDisplayLcd, 0, 0, "%cAvailable FW: %s", selectedChar, availableFwVersion);
        /* clear display and print to stored node FW version UART */
        Display_print2(hDisplaySerial, 0, 0, "\033[2J \033[0;0H%cAvailable FW: %s", selectedChar, availableFwVersion);

        /* if there are any nodes print the table header */
        if(nodePointer->address != 0)
        {
            /* print table header to LCD*/
            Display_print0(hDisplayLcd, 1, 0, "Nodes Value SW  RSSI");
            /* print table header  to LCD*/
            Display_print0(hDisplaySerial, 1, 0, "Nodes   Value   SW   RSSI");
        }

        /* Start on the third line */
        currentLcdLine = 2;

        /* Write one line per node */
        while ((nodePointer < &knownSensorNodes[CONCENTRATOR_MAX_NODES]) &&
              (nodePointer->address != 0) &&
              (currentLcdLine < CONCENTRATOR_DISPLAY_LINES))
        {
            if ( (currentLcdLine - 1) == selectedNode )
            {
                selectedChar = '*';
            }
            else
            {
                selectedChar = ' ';
            }
            /* print to LCD */
            Display_print5(hDisplayLcd, currentLcdLine, 0, "%c0x%02x  %04d  %d   %04d", selectedChar,
                    nodePointer->address, nodePointer->latestAdcValue, nodePointer->button,
                    nodePointer->latestRssi);

            /* print to UART */
            Display_print5(hDisplaySerial, 0, 0, "%c0x%02x    %04d    %d    %04d", selectedChar,
                    nodePointer->address, nodePointer->latestAdcValue, nodePointer->button,
                    nodePointer->latestRssi);

            nodePointer++;
            currentLcdLine++;
        }

        //if we have some selected nodes print the FW version
        if (selectedNode > 0)
        {
            switch (knownSensorNodes[selectedNode-1].nodeState)
            {
            case NODE_STATE_REQ_FW_UPDATE:
                /* print current FW version */
                Display_print1(hDisplayLcd, currentLcdLine, 0, "FW: %s", fwUpdatingStr);
                Display_print1(hDisplaySerial, 0, 0, "FW: %s", fwUpdatingStr);
                break;

            case NODE_STATE_REQ_FW_UPDATE_FAILED:
                /* print current FW version */
                Display_print1(hDisplayLcd, currentLcdLine, 0, "FW: %s", fwUpateFailedStr);
                Display_print1(hDisplaySerial, 0, 0, "FW: %s", fwUpateFailedStr);
                break;

            case NODE_STATE_GET_FW_VER:
            case NODE_STATE_GET_FW_VER_FAILED:
            case NODE_STATE_FW_UPDATING:
            case NODE_STATE_RUNNING:
            default:
                /* print current FW version */
                Display_print1(hDisplayLcd, currentLcdLine, 0, "FW: %s", knownSensorNodes[selectedNode-1].fwVersion);
                Display_print1(hDisplaySerial, 0, 0, "FW: %s", knownSensorNodes[selectedNode-1].fwVersion);
                break;
            }

            if (WSNProtocol_isPending())
            {
                /* print to LCD */
                Display_print0(hDisplayLcd, currentLcdLine+1, 0, "Press BTN-2 to cancel node operation");
                /* print to UART */
                Display_print0(hDisplaySerial, 0, 0, "Press BTN-2 to cancel node operation");
            }
            else if (fwOadInProgress)
            {
                /* print to LCD */
                Display_print0(hDisplayLcd, currentLcdLine+1, 0, "Press BTN-2 to cancel FW update");
                /* print to UART */
                Display_print0(hDisplaySerial, 0, 0, "Press BTN-2 to cancel FW update");
            }
        }
        else
        {
            /* print to LCD */
            Display_print0(hDisplayLcd, currentLcdLine+1, 0, "Press BTN-2 to update available node FW");
            /* print to UART */
            Display_print0(hDisplaySerial, 0, 0, "Press BTN-2 to update available node FW");
        }
    }
}

/*
 *  ======== buttonCallback ========
 *  Pin interrupt Callback function board buttons configured in the pinTable.
 */
void buttonCallback(PIN_Handle handle, PIN_Id pinId)
{
    /* Debounce logic, only toggle if the button is still pushed (low) */
    CPUdelay(8000*50);

    if ((PIN_getInputValue(Board_BUTTON0) == 0) & (PIN_getInputValue(Board_BUTTON1) == 0))
    {
#ifdef USE_BIM
        //stop button interrupts while checking ble bootload button hold
        PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON0 | PIN_IRQ_DIS);
        PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON1 | PIN_IRQ_DIS);
        //start BLE bootloader timer to check pins
        Clock_start(bleBootloadBtnCheckClockHandle);
#endif
    }
    else if (PIN_getInputValue(Board_BUTTON0) == 0)
    {
        uint8_t numOfNodes;
        for(numOfNodes = 0; ((knownSensorNodes[numOfNodes].address != 0) && (numOfNodes < CONCENTRATOR_MAX_NODES)); numOfNodes++);

        //select node
        selectedNode++;
        if ( selectedNode > numOfNodes )
        {
            selectedNode = 0;
        }

        //trigger LCD update
        Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_UPDATE_DISPLAY);
    }
    else if (PIN_getInputValue(Board_BUTTON1) == 0)
    {
        if(selectedNode == 0)
        {
            Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_USD_START);
        }
        else
        {
            Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_NODE_BTN);
        }
    }
}

#ifdef USE_BIM
static void bleBtnCheckTimerCallback(UArg arg0)
{
    static uint32_t btnHoldCnt = 0;

    if ((PIN_getInputValue(Board_BUTTON0) == 0) & (PIN_getInputValue(Board_BUTTON1) == 0))
    {
        if ( (++btnHoldCnt * NODE_BLE_BOOTLODER_BUTTON_CHECK_TIMER_MS) >
            (NODE_BLE_BOOTLODER_BUTTON_CHECK_DURATION_S*1000))
        {
            Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_FACTORY_RESET);
            btnHoldCnt = 0;
        }
        else
        {
            Clock_start(bleBootloadBtnCheckClockHandle);
        }
    }
    else
    {
        btnHoldCnt = 0;
        /* Re-enable interrupts to detect button release. */
        PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON0 | PIN_IRQ_NEGEDGE);
        PIN_setConfig(buttonPinHandle, PIN_BM_IRQ, Board_BUTTON1 | PIN_IRQ_NEGEDGE);
    }
}
#endif //USE_BIM
